#include <QByteArray>
#include <QXmlAttributes>
#include <QXmlStreamReader>
#include <QString>
#include <QDebug>
#include <QDateTime>

#include "_Interface/_global_datastruct.h"
#include "backinstance.h"
#include "Package/oxfordlasersensordll_global.h"
#include "Package/simpletcpsocketdll_global.h"
#include "Package/SimpleRoboticMath/ConsoleMain/simpleroboticmath.h"

void backinstance::toRobotKRLSendLoopRun(){
    double TimeKey;
    static double RobotSendTimeKey = 0;
    TimeKey = QDateTime::currentDateTime().toMSecsSinceEpoch();

    if(this->pRobotDAQData->bConnected == true){
        char tempSend[64];
        QByteArray Data;
        QString SendString;
        QXmlStreamWriter stream(&SendString);
        memset(tempSend,'\0',strlen(tempSend));
        switch (this->WorkModel) {
            case ENUM_WorkModel::Null:
                stream.writeStartElement("Ext");
                    stream.writeStartElement("SeamData");
                        stream.writeStartElement("Flag");
                            stream.writeTextElement("Ask","0");
                            stream.writeTextElement("Move","0");
                        stream.writeEndElement();
                        stream.writeStartElement("Init");
                            stream.writeTextElement("Xs","0");
                            stream.writeTextElement("Ys","0");
                            stream.writeTextElement("Zs","0");
                        stream.writeEndElement();
                        stream.writeStartElement("End");
                            stream.writeTextElement("Xe","0");
                            stream.writeTextElement("Ye","0");
                            stream.writeTextElement("Ze","0");
                        stream.writeEndElement();
                    stream.writeEndElement();
                stream.writeEndElement();
            break;
            case ENUM_WorkModel::Calibration:
                stream.writeStartElement("Ext");
                    stream.writeStartElement("SeamData");
                        stream.writeStartElement("Flag");
                            stream.writeTextElement("Ask","1");
                            stream.writeTextElement("Move","0");
                        stream.writeEndElement();
                        stream.writeStartElement("Init");
                            stream.writeTextElement("Xs","0");
                            stream.writeTextElement("Ys","0");
                            stream.writeTextElement("Zs","0");
                        stream.writeEndElement();
                        stream.writeStartElement("End");
                            stream.writeTextElement("Xe","0");
                            stream.writeTextElement("Ye","0");
                            stream.writeTextElement("Ze","0");
                        stream.writeEndElement();
                    stream.writeEndElement();
                stream.writeEndElement();
                break;
            case ENUM_WorkModel::SeamFinding:
                stream.writeStartElement("Ext");
                    stream.writeStartElement("SeamData");
                        stream.writeStartElement("Flag");
                            stream.writeTextElement("Ask","2");
                            sprintf(tempSend,"%d", this->pSeamFindingData->toRobotMove);
                            stream.writeTextElement("Move",tempSend);
                        stream.writeEndElement();
                        stream.writeStartElement("Init");
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->StartPos[0]);
                            stream.writeTextElement("Xs",tempSend);
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->StartPos[1]);
                            stream.writeTextElement("Ys",tempSend);
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->StartPos[2]);
                            stream.writeTextElement("Zs",tempSend);
                        stream.writeEndElement();
                        stream.writeStartElement("End");
                            stream.writeTextElement("Xe","0");
                            stream.writeTextElement("Ye","0");
                            stream.writeTextElement("Ze","0");
                        stream.writeEndElement();
                    stream.writeEndElement();
                stream.writeEndElement();
                break;
            case ENUM_WorkModel::SeamFindingStart2End:
                stream.writeStartElement("Ext");
                    stream.writeStartElement("SeamData");
                        stream.writeStartElement("Flag");
                            stream.writeTextElement("Ask","3");
                            sprintf(tempSend,"%d", this->pSeamFindingData->toRobotMove);
                            stream.writeTextElement("Move",tempSend);
                        stream.writeEndElement();
                        stream.writeStartElement("Init");
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->StartPos[0]);
                            stream.writeTextElement("Xs",tempSend);
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->StartPos[1]);
                            stream.writeTextElement("Ys",tempSend);
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->StartPos[2]);
                            stream.writeTextElement("Zs",tempSend);
                        stream.writeEndElement();
                        stream.writeStartElement("End");
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->EndPos[0]);
                            stream.writeTextElement("Xe",tempSend);
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->EndPos[1]);
                            stream.writeTextElement("Ye",tempSend);
                            sprintf(tempSend,"%.3f", this->pSeamFindingData->EndPos[2]);
                            stream.writeTextElement("Ze",tempSend);
                        stream.writeEndElement();
                    stream.writeEndElement();
                stream.writeEndElement();
                break;
            case ENUM_WorkModel::SeamTracking:
                stream.writeStartElement("Ext");
                    stream.writeStartElement("SeamData");
                        stream.writeStartElement("Flag");
                            stream.writeTextElement("Ask","4");
                            stream.writeTextElement("Move","0");
                        stream.writeEndElement();
                        stream.writeStartElement("Init");
                            stream.writeTextElement("Xs","0");
                            stream.writeTextElement("Ys","0");
                            stream.writeTextElement("Zs","0");
                        stream.writeEndElement();
                        stream.writeStartElement("End");
                            stream.writeTextElement("Xe","0");
                            stream.writeTextElement("Ye","0");
                            stream.writeTextElement("Ze","0");
                        stream.writeEndElement();
                    stream.writeEndElement();
                stream.writeEndElement();
                break;
        }
        if(this->pRobotDAQData->bSend == true || this->pRobotDAQData->bFirstConnected == true){
            Data = SendString.toLocal8Bit();
            COM_clientSendData(this->EKRLClient,Data);
            this->pRobotDAQData->bSend = false;
            this->pRobotDAQData->bFirstConnected = false;
            RobotSendTimeKey = TimeKey;
        }
        else if(this->pRobotDAQData->bSend == false || this->pRobotDAQData->bFirstConnected == false){
            if(TimeKey - RobotSendTimeKey > 50){
                Data = SendString.toLocal8Bit();
                COM_clientSendData(this->EKRLClient,Data);
                this->pRobotDAQData->bSend = false;
                this->pRobotDAQData->bFirstConnected = false;
                RobotSendTimeKey = TimeKey;
            }
        }
    }
}

void backinstance::toRobotKRLReceiveLoopRun(){
    if(this->pRobotDAQData->bConnected == true){

        QByteArray Data;
        QXmlStreamReader mQXmlStreamReader;
        COM_clientReadData(this->EKRLClient,Data);

        mQXmlStreamReader.addData(Data);
        while(!mQXmlStreamReader.atEnd()){
            if(mQXmlStreamReader.isStartElement()){
                if(mQXmlStreamReader.name() == "X"){
                    QString  tempQString;
                    tempQString = (mQXmlStreamReader.readElementText());
                    this->pRobotDAQData->KRLRobotPos[0] = tempQString.toDouble();
                }
                else if(mQXmlStreamReader.name() == "Y"){
                    QString  tempQString;
                    tempQString = (mQXmlStreamReader.readElementText());
                    this->pRobotDAQData->KRLRobotPos[1] = tempQString.toDouble();
                }
                else if(mQXmlStreamReader.name() == "Z"){
                    QString  tempQString;
                    tempQString = (mQXmlStreamReader.readElementText());
                    this->pRobotDAQData->KRLRobotPos[2] = tempQString.toDouble();
                }
                else if(mQXmlStreamReader.name() == "A"){
                    QString  tempQString;
                    tempQString = (mQXmlStreamReader.readElementText());
                    this->pRobotDAQData->KRLRobotPos[3] = tempQString.toDouble();
                }
                else if(mQXmlStreamReader.name() == "B"){
                    QString  tempQString;
                    tempQString = (mQXmlStreamReader.readElementText());
                    this->pRobotDAQData->KRLRobotPos[4] = tempQString.toDouble();
                }
                else if(mQXmlStreamReader.name() == "C"){
                    QString  tempQString;
                    tempQString = (mQXmlStreamReader.readElementText());
                    this->pRobotDAQData->KRLRobotPos[5] = tempQString.toDouble();
                }
                else if(mQXmlStreamReader.name() == "SeamScan"){
                    QString  tempQString;
                    tempQString = (mQXmlStreamReader.readElementText());
                    this->pSeamFindingData->frRobotSeamScan = tempQString.toInt();
                    this->pRobotDAQData->bSend = true;
                }
            }
            mQXmlStreamReader.readNext();
        }
        mQXmlStreamReader.clear();
    }
}

void backinstance::SeamTechLoopRun(){
    if(this->WorkType == ENUM_WorkType::Auto){
        if (this->pSeamFindingData->frRobotSeamScan == 0){
            this->WorkModel = ENUM_WorkModel::Null;
        }
        else if (this->pSeamFindingData->frRobotSeamScan >= 10 && this->pSeamFindingData->frRobotSeamScan < 20){
            this->WorkModel = ENUM_WorkModel::Calibration;
            this->onSeamCalibrationProcess_Auto();
        }
        else if(this->pSeamFindingData->frRobotSeamScan >= 30 && this->pSeamFindingData->frRobotSeamScan < 40){
            this->WorkModel = ENUM_WorkModel::SeamFindingStart2End;
            this->onSeamFindingStart2EndProcess_Auto();
        }
    }
    else{
    }
}
